import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 获取与拼接默认路径
    robot_one_navigation_dir = get_package_share_directory('robot_one_navigation')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(robot_one_navigation_dir, 'config', 'rviz', 'robot_one_navigation.rviz')
    
    # 自定义行为树文件路径
    custom_bt_xml_path = os.path.join(robot_one_navigation_dir, 'behavior_trees', 'navigate_to_pose_once.xml')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration('map', default=os.path.join(robot_one_navigation_dir, 'maps', 'map1.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration('params_file', default=os.path.join(robot_one_navigation_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                                             description='Use simulation (Gazebo) clock if true'),
        launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path,
                                             description='Full path to map file to load'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                                             description='Full path to param file to load'),
        launch.actions.DeclareLaunchArgument('default_bt_xml_filename', default_value=custom_bt_xml_path,
                                             description='Full path to behavior tree xml file to load'),

        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path,
                'default_bt_xml_filename': custom_bt_xml_path  # 添加自定义行为树文件路径
            }.items(),
        ),
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),

        # 初始化位姿节点 
        launch_ros.actions.Node(
            package='robot_one_navigation',
            executable='robot_pose_init',         
            name='robot_pose_init',
            output='screen'),
    ])